#include "../include/ScaramuzzaCamera.h"
#include "../include/Utils.h"

#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <cmath>
#include <cstdio>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/SVD>
#include <iomanip>
#include <iostream>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/imgproc/imgproc.hpp>

Eigen::VectorXd polyfit(Eigen::VectorXd &xVec, Eigen::VectorXd &yVec,
                        int poly_order)
{
  assert(poly_order > 0);
  assert(xVec.size() > poly_order);
  assert(xVec.size() == yVec.size());

  Eigen::MatrixXd A(xVec.size(), poly_order + 1);
  Eigen::VectorXd B(xVec.size());

  for (int i = 0; i < xVec.size(); ++i)
  {
    const double x = xVec(i);
    const double y = yVec(i);

    double x_pow_k = 1.0;

    for (int k = 0; k <= poly_order; ++k)
    {
      A(i, k) = x_pow_k;
      x_pow_k *= x;
    }

    B(i) = y;
  }

  Eigen::JacobiSVD<Eigen::MatrixXd> svd(
      A, Eigen::ComputeThinU | Eigen::ComputeThinV);
  Eigen::VectorXd x = svd.solve(B);

  return x;
}

OCAMCamera::Parameters::Parameters()
    : Camera::Parameters(SCARAMUZZA),
      m_C(0.0),
      m_D(0.0),
      m_E(0.0),
      m_center_x(0.0),
      m_center_y(0.0)
{
  memset(m_poly, 0, sizeof(double) * SCARAMUZZA_POLY_SIZE);
  memset(m_inv_poly, 0, sizeof(double) * SCARAMUZZA_INV_POLY_SIZE);
}

bool OCAMCamera::Parameters::readFromYamlFile(const std::string &filename)
{
  cv::FileStorage fs(filename, cv::FileStorage::READ);

  if (!fs.isOpened())
  {
    return false;
  }

  if (!fs["model_type"].isNone())
  {
    std::string sModelType;
    fs["model_type"] >> sModelType;

    if (!boost::iequals(sModelType, "scaramuzza"))
    {
      return false;
    }
  }

  m_modelType = SCARAMUZZA;
  fs["camera_name"] >> m_cameraName;
  m_imageWidth = static_cast<int>(fs["image_width"]);
  m_imageHeight = static_cast<int>(fs["image_height"]);

  cv::FileNode n = fs["poly_parameters"];
  for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++)
    m_poly[i] = static_cast<double>(
        n[std::string("p") + boost::lexical_cast<std::string>(i)]);

  n = fs["inv_poly_parameters"];
  for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
    m_inv_poly[i] = static_cast<double>(
        n[std::string("p") + boost::lexical_cast<std::string>(i)]);

  n = fs["affine_parameters"];
  m_C = static_cast<double>(n["ac"]);
  m_D = static_cast<double>(n["ad"]);
  m_E = static_cast<double>(n["ae"]);

  m_center_x = static_cast<double>(n["cx"]);
  m_center_y = static_cast<double>(n["cy"]);

  return true;
}

void OCAMCamera::Parameters::writeToYamlFile(
    const std::string &filename) const
{
  cv::FileStorage fs(filename, cv::FileStorage::WRITE);

  fs << "model_type"
     << "scaramuzza";
  fs << "camera_name" << m_cameraName;
  fs << "image_width" << m_imageWidth;
  fs << "image_height" << m_imageHeight;

  fs << "poly_parameters";
  fs << "{";
  for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++)
    fs << std::string("p") + boost::lexical_cast<std::string>(i) << m_poly[i];
  fs << "}";

  fs << "inv_poly_parameters";
  fs << "{";
  for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
    fs << std::string("p") + boost::lexical_cast<std::string>(i)
       << m_inv_poly[i];
  fs << "}";

  fs << "affine_parameters";
  fs << "{"
     << "ac" << m_C << "ad" << m_D << "ae" << m_E << "cx" << m_center_x << "cy"
     << m_center_y << "}";

  fs.release();
}

OCAMCamera::Parameters &OCAMCamera::Parameters::operator=(
    const OCAMCamera::Parameters &other)
{
  if (this != &other)
  {
    m_modelType = other.m_modelType;
    m_cameraName = other.m_cameraName;
    m_imageWidth = other.m_imageWidth;
    m_imageHeight = other.m_imageHeight;
    m_C = other.m_C;
    m_D = other.m_D;
    m_E = other.m_E;
    m_center_x = other.m_center_x;
    m_center_y = other.m_center_y;

    memcpy(m_poly, other.m_poly, sizeof(double) * SCARAMUZZA_POLY_SIZE);
    memcpy(m_inv_poly, other.m_inv_poly,
           sizeof(double) * SCARAMUZZA_INV_POLY_SIZE);
  }

  return *this;
}

std::ostream &operator<<(std::ostream &out,
                         const OCAMCamera::Parameters &params)
{
  out << "Camera Parameters:" << std::endl;
  out << "    model_type "
      << "scaramuzza" << std::endl;
  out << "   camera_name " << params.m_cameraName << std::endl;
  out << "   image_width " << params.m_imageWidth << std::endl;
  out << "  image_height " << params.m_imageHeight << std::endl;

  out << std::fixed << std::setprecision(10);

  out << "Poly Parameters" << std::endl;
  for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++)
    out << std::string("p") + boost::lexical_cast<std::string>(i) << ": "
        << params.m_poly[i] << std::endl;

  out << "Inverse Poly Parameters" << std::endl;
  for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
    out << std::string("p") + boost::lexical_cast<std::string>(i) << ": "
        << params.m_inv_poly[i] << std::endl;

  out << "Affine Parameters" << std::endl;
  out << "            ac " << params.m_C << std::endl
      << "            ad " << params.m_D << std::endl
      << "            ae " << params.m_E << std::endl;
  out << "            cx " << params.m_center_x << std::endl
      << "            cy " << params.m_center_y << std::endl;

  return out;
}

OCAMCamera::OCAMCamera() : m_inv_scale(0.0) {}

OCAMCamera::OCAMCamera(const OCAMCamera::Parameters &params)
    : mParameters(params)
{
  m_inv_scale = 1.0 / (params.C() - params.D() * params.E());
}

Camera::ModelType OCAMCamera::modelType(void) const
{
  return mParameters.modelType();
}

const std::string &OCAMCamera::cameraName(void) const
{
  return mParameters.cameraName();
}

int OCAMCamera::imageWidth(void) const { return mParameters.imageWidth(); }

int OCAMCamera::imageHeight(void) const { return mParameters.imageHeight(); }

void OCAMCamera::estimateIntrinsics(
    const cv::Size &boardSize,
    const std::vector<std::vector<cv::Point3f>> &objectPoints,
    const std::vector<std::vector<cv::Point2f>> &imagePoints)
{
  // std::cout << "OCAMCamera::estimateIntrinsics - NOT IMPLEMENTED" <<
  // std::endl;
  // throw std::string("OCAMCamera::estimateIntrinsics - NOT IMPLEMENTED");

  // Reference: Page 30 of
  // " Scaramuzza, D. Omnidirectional Vision: from Calibration to Robot Motion
  // Estimation, ETH Zurich. Thesis no. 17635."
  // http://e-collection.library.ethz.ch/eserv/eth:30301/eth-30301-02.pdf
  // Matlab code: calibrate.m

  // First, estimate every image's extrinsics parameters
  std::vector<Eigen::Matrix3d> RList;
  std::vector<Eigen::Vector3d> TList;

  RList.reserve(imagePoints.size());
  TList.reserve(imagePoints.size());

  // i-th image
  for (size_t image_index = 0; image_index < imagePoints.size();
       ++image_index)
  {
    const std::vector<cv::Point3f> &objPts = objectPoints.at(image_index);
    const std::vector<cv::Point2f> &imgPts = imagePoints.at(image_index);

    assert(objPts.size() == imgPts.size());
    assert(objPts.size() ==
           static_cast<unsigned int>(boardSize.width * boardSize.height));

    Eigen::MatrixXd M(objPts.size(), 6);

    for (size_t corner_index = 0; corner_index < objPts.size();
         ++corner_index)
    {
      double X = objPts.at(corner_index).x;
      double Y = objPts.at(corner_index).y;
      assert(objPts.at(corner_index).z == 0.0);

      double u = imgPts.at(corner_index).x;
      double v = imgPts.at(corner_index).y;

      M(corner_index, 0) = -v * X;
      M(corner_index, 1) = -v * Y;
      M(corner_index, 2) = u * X;
      M(corner_index, 3) = u * Y;
      M(corner_index, 4) = -v;
      M(corner_index, 5) = u;
    }

    Eigen::JacobiSVD<Eigen::MatrixXd> svd(
        M, Eigen::ComputeFullU | Eigen::ComputeFullV);
    assert(svd.matrixV().cols() == 6);
    Eigen::VectorXd h = -svd.matrixV().col(5);

    // scaled version of R and T
    const double sr11 = h(0);
    const double sr12 = h(1);
    const double sr21 = h(2);
    const double sr22 = h(3);
    const double st1 = h(4);
    const double st2 = h(5);

    const double AA = square(sr11 * sr12 + sr21 * sr22);
    const double BB = square(sr11) + square(sr21);
    const double CC = square(sr12) + square(sr22);

    const double sr32_squared_1 =
        (-(CC - BB) + sqrt(square(CC - BB) + 4.0 * AA)) / 2.0;
    const double sr32_squared_2 =
        (-(CC - BB) - sqrt(square(CC - BB) + 4.0 * AA)) / 2.0;

    // printf("rst = %.12f\n", sr32_squared_1*sr32_squared_1 +
    // (CC-BB)*sr32_squared_1 - AA);

    std::vector<double> sr32_squared_values;
    if (sr32_squared_1 > 0)
      sr32_squared_values.push_back(sr32_squared_1);
    if (sr32_squared_2 > 0)
      sr32_squared_values.push_back(sr32_squared_2);
    assert(!sr32_squared_values.empty());

    std::vector<double> sr32_values;
    std::vector<double> sr31_values;
    for (auto sr32_squared : sr32_squared_values)
    {
      for (int sign = -1; sign <= 1; sign += 2)
      {
        const double sr32 = static_cast<double>(sign) * std::sqrt(sr32_squared);
        sr32_values.push_back(sr32);
        if (sr32_squared == 0.0)
        {
          // sr31 can be calculated through norm equality,
          // but it has positive and negative posibilities
          // positive one
          sr31_values.push_back(std::sqrt(CC - BB));
          // negative one
          sr32_values.push_back(sr32);
          sr31_values.push_back(-std::sqrt(CC - BB));

          break; // skip the same situation
        }
        else
        {
          // sr31 can be calculated throught dot product == 0
          sr31_values.push_back(-(sr11 * sr12 + sr21 * sr22) / sr32);
        }
      }
    }

    // std::cout << "h= " << std::setprecision(12) << h.transpose() <<
    // std::endl;
    // std::cout << "length: " << sr32_values.size() << " & " <<
    // sr31_values.size() << std::endl;

    assert(!sr31_values.empty());
    assert(sr31_values.size() == sr32_values.size());

    std::vector<Eigen::Matrix3d> H_values;
    for (size_t i = 0; i < sr31_values.size(); ++i)
    {
      const double sr31 = sr31_values.at(i);
      const double sr32 = sr32_values.at(i);
      const double lambda = 1.0 / sqrt(sr11 * sr11 + sr21 * sr21 + sr31 * sr31);
      Eigen::Matrix3d H;
      H.setZero();
      H(0, 0) = sr11;
      H(0, 1) = sr12;
      H(0, 2) = st1;
      H(1, 0) = sr21;
      H(1, 1) = sr22;
      H(1, 2) = st2;
      H(2, 0) = sr31;
      H(2, 1) = sr32;
      H(2, 2) = 0;

      H_values.push_back(lambda * H);
      H_values.push_back(-lambda * H);
    }

    for (auto &H : H_values)
    {
      // std::cout << "H=\n" << H << std::endl;
      Eigen::Matrix3d R;
      R.col(0) = H.col(0);
      R.col(1) = H.col(1);
      R.col(2) = H.col(0).cross(H.col(1));
      // std::cout << "R33 = " << R(2,2) << std::endl;
    }

    std::vector<Eigen::Matrix3d> H_candidates;

    for (auto &H : H_values)
    {
      Eigen::MatrixXd A_mat(2 * imagePoints.at(image_index).size(), 4);
      Eigen::VectorXd B_vec(2 * imagePoints.at(image_index).size());
      A_mat.setZero();
      B_vec.setZero();

      size_t line_index = 0;

      // iterate images
      const double &r11 = H(0, 0);
      const double &r12 = H(0, 1);
      // const double& r13 = H(0,2);
      const double &r21 = H(1, 0);
      const double &r22 = H(1, 1);
      // const double& r23 = H(1,2);
      const double &r31 = H(2, 0);
      const double &r32 = H(2, 1);
      // const double& r33 = H(2,2);
      const double &t1 = H(0);
      const double &t2 = H(1);

      // iterate chessboard corners in the image
      for (size_t j = 0; j < imagePoints.at(image_index).size(); ++j)
      {
        assert(line_index == 2 * j);

        const double &X = objectPoints.at(image_index).at(j).x;
        const double &Y = objectPoints.at(image_index).at(j).y;
        const double &u = imagePoints.at(image_index).at(j).x;
        const double &v = imagePoints.at(image_index).at(j).y;

        double A = r21 * X + r22 * Y + t2;
        double B = v * (r31 * X + r32 * Y);
        double C = r11 * X + r12 * Y + t1;
        double D = u * (r31 * X + r32 * Y);
        double rou = std::sqrt(u * u + v * v);

        A_mat(line_index + 0, 0) = A;
        A_mat(line_index + 1, 0) = C;
        A_mat(line_index + 0, 1) = A * rou;
        A_mat(line_index + 1, 1) = C * rou;
        A_mat(line_index + 0, 2) = A * rou * rou;
        A_mat(line_index + 1, 2) = C * rou * rou;

        A_mat(line_index + 0, 3) = -v;
        A_mat(line_index + 1, 3) = -u;
        B_vec(line_index + 0) = B;
        B_vec(line_index + 1) = D;

        line_index += 2;
      }

      assert(line_index == static_cast<unsigned int>(A_mat.rows()));

      // pseudo-inverse for polynomial parameters and all t3s
      {
        Eigen::JacobiSVD<Eigen::MatrixXd> svd(
            A_mat, Eigen::ComputeThinU | Eigen::ComputeThinV);

        Eigen::VectorXd x = svd.solve(B_vec);

        // std::cout << "x(poly and t3) = " << x << std::endl;

        if (x(2) > 0 && x(3) > 0)
        {
          H_candidates.push_back(H);
        }
      }
    }

    // printf("H_candidates.size()=%zu\n", H_candidates.size());
    assert(H_candidates.size() == 1);

    Eigen::Matrix3d &H = H_candidates.front();

    Eigen::Matrix3d R;
    R.col(0) = H.col(0);
    R.col(1) = H.col(1);
    R.col(2) = H.col(0).cross(H.col(1));

    Eigen::Vector3d T = H.col(2);
    RList.push_back(R);
    TList.push_back(T);

    // std::cout << "#" << image_index << " frame" << " R =" << R << " \nT = "
    // << T.transpose() << std::endl;
  }

  // Second, estimate camera intrinsic parameters and all t3
  Eigen::MatrixXd A_mat(2 * imagePoints.size() * imagePoints.at(0).size(),
                        SCARAMUZZA_POLY_SIZE - 1 + imagePoints.size());
  Eigen::VectorXd B_vec(2 * imagePoints.size() * imagePoints.at(0).size());
  A_mat.setZero();
  B_vec.setZero();

  size_t line_index = 0;

  // iterate images
  for (size_t i = 0; i < imagePoints.size(); ++i)
  {
    const double &r11 = RList.at(i)(0, 0);
    const double &r12 = RList.at(i)(0, 1);
    // const double& r13 = RList.at(i)(0,2);
    const double &r21 = RList.at(i)(1, 0);
    const double &r22 = RList.at(i)(1, 1);
    // const double& r23 = RList.at(i)(1,2);
    const double &r31 = RList.at(i)(2, 0);
    const double &r32 = RList.at(i)(2, 1);
    // const double& r33 = RList.at(i)(2,2);
    const double &t1 = TList.at(i)(0);
    const double &t2 = TList.at(i)(1);

    // iterate chessboard corners in the image
    for (size_t j = 0; j < imagePoints.at(i).size(); ++j)
    {
      assert(line_index == 2 * (i * imagePoints.at(0).size() + j));

      const double &X = objectPoints.at(i).at(j).x;
      const double &Y = objectPoints.at(i).at(j).y;
      const double &u = imagePoints.at(i).at(j).x;
      const double &v = imagePoints.at(i).at(j).y;

      double A = r21 * X + r22 * Y + t2;
      double B = v * (r31 * X + r32 * Y);
      double C = r11 * X + r12 * Y + t1;
      double D = u * (r31 * X + r32 * Y);
      double rou = std::sqrt(u * u + v * v);

      for (int k = 1; k <= SCARAMUZZA_POLY_SIZE - 1; ++k)
      {
        double pow_rou = 0.0;
        if (k == 1)
        {
          pow_rou = 1.0;
        }
        else
        {
          pow_rou = std::pow(rou, k);
        }

        A_mat(line_index + 0, k - 1) = A * pow_rou;
        A_mat(line_index + 1, k - 1) = C * pow_rou;
      }

      A_mat(line_index + 0, SCARAMUZZA_POLY_SIZE - 1 + i) = -v;
      A_mat(line_index + 1, SCARAMUZZA_POLY_SIZE - 1 + i) = -u;
      B_vec(line_index + 0) = B;
      B_vec(line_index + 1) = D;

      line_index += 2;
    }
  }

  assert(line_index == static_cast<unsigned int>(A_mat.rows()));

  Eigen::Matrix<double, SCARAMUZZA_POLY_SIZE, 1> poly_coeff;
  // pseudo-inverse for polynomial parameters and all t3s
  {
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(
        A_mat, Eigen::ComputeThinU | Eigen::ComputeThinV);

    Eigen::VectorXd x = svd.solve(B_vec);

    poly_coeff[0] = x(0);
    poly_coeff[1] = 0.0;
    for (int i = 1; i < poly_coeff.size() - 1; ++i)
    {
      poly_coeff[i + 1] = x(i);
    }
    assert(x.size() ==
           static_cast<unsigned int>(SCARAMUZZA_POLY_SIZE - 1 + TList.size()));
  }

  Parameters params = getParameters();

  // Affine matrix A is constructed as [C D; E 1]
  params.C() = 1.0;
  params.D() = 0.0;
  params.E() = 0.0;

  params.center_x() = params.imageWidth() / 2.0;
  params.center_y() = params.imageHeight() / 2.0;

  for (size_t i = 0; i < SCARAMUZZA_POLY_SIZE; ++i)
  {
    params.poly(i) = poly_coeff[i];
  }

  // params.poly(0) = -216.9657476318;
  // params.poly(1) = 0.0;
  // params.poly(2) = 0.0017866911;
  // params.poly(3) = -0.0000019866;
  // params.poly(4) =  0.0000000077;

  // inv_poly
  {
    std::vector<double> rou_vec;
    std::vector<double> z_vec;
    for (double rou = 0.0;
         rou <= (params.imageWidth() + params.imageHeight()) / 2; rou += 0.1)
    {
      double rou_pow_k = 1.0;
      double z = 0.0;

      for (int k = 0; k < SCARAMUZZA_POLY_SIZE; k++)
      {
        z += rou_pow_k * params.poly(k);
        rou_pow_k *= rou;
      }

      rou_vec.push_back(rou);
      z_vec.push_back(z);
    }

    assert(rou_vec.size() == z_vec.size());
    Eigen::VectorXd xVec(rou_vec.size());
    Eigen::VectorXd yVec(rou_vec.size());

    for (size_t i = 0; i < rou_vec.size(); ++i)
    {
      xVec(i) = std::atan2(-z_vec.at(i), rou_vec.at(i));
      yVec(i) = rou_vec.at(i);
    }

    // use lower order poly to eliminate over-fitting cause by noisy/inaccurate
    // data
    const int poly_fit_order = 4;
    Eigen::VectorXd inv_poly_coeff = polyfit(xVec, yVec, poly_fit_order);

    for (int i = 0; i <= poly_fit_order; ++i)
    {
      params.inv_poly(i) = inv_poly_coeff(i);
    }
  }

  setParameters(params);

  std::cout << "initial params:\n"
            << params << std::endl;
}

/**
 * \brief Lifts a point from the image plane to the unit sphere
 *
 * \param p image coordinates
 * \param P coordinates of the point on the sphere
 */
void OCAMCamera::liftSphere(const Eigen::Vector2d &p,
                            Eigen::Vector3d &P) const
{
  liftProjective(p, P);
  P.normalize();
}

/**
 * \brief Lifts a point from the image plane to its projective ray
 *
 * \param p image coordinates
 * \param P coordinates of the projective ray
 */
void OCAMCamera::liftProjective(const Eigen::Vector2d &p,
                                Eigen::Vector3d &P) const
{
  // Relative to Center
  Eigen::Vector2d xc(p[0] - mParameters.center_x(),
                     p[1] - mParameters.center_y());

  // Affine Transformation
  // xc_a = inv(A) * xc;
  Eigen::Vector2d xc_a(
      m_inv_scale * (xc[0] - mParameters.D() * xc[1]),
      m_inv_scale * (-mParameters.E() * xc[0] + mParameters.C() * xc[1]));

  double phi = std::sqrt(xc_a[0] * xc_a[0] + xc_a[1] * xc_a[1]);
  double phi_i = 1.0;
  double z = 0.0;

  for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++)
  {
    z += phi_i * mParameters.poly(i);
    phi_i *= phi;
  }

  P << xc[0], xc[1], -z;
}

/**
 * \brief Project a 3D point (\a x,\a y,\a z) to the image plane in (\a u,\a v)
 *
 * \param P 3D point coordinates
 * \param p return value, contains the image point coordinates
 */
void OCAMCamera::spaceToPlane(const Eigen::Vector3d &P,
                              Eigen::Vector2d &p) const
{
  double norm = std::sqrt(P[0] * P[0] + P[1] * P[1]);
  double theta = std::atan2(-P[2], norm);
  double rho = 0.0;
  double theta_i = 1.0;

  for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
  {
    rho += theta_i * mParameters.inv_poly(i);
    theta_i *= theta;
  }

  double invNorm = 1.0 / norm;
  Eigen::Vector2d xn(P[0] * invNorm * rho, P[1] * invNorm * rho);

  p << xn[0] * mParameters.C() + xn[1] * mParameters.D() +
           mParameters.center_x(),
      xn[0] * mParameters.E() + xn[1] + mParameters.center_y();
}

/**
 * \brief Projects an undistorted 2D point p_u to the image plane
 *
 * \param p_u 2D point coordinates
 * \return image point coordinates
 */
void OCAMCamera::undistToPlane(const Eigen::Vector2d &p_u,
                               Eigen::Vector2d &p) const
{
  Eigen::Vector3d P(p_u[0], p_u[1], 1.0);
  spaceToPlane(P, p);
}

#if 0
void
OCAMCamera::initUndistortMap(cv::Mat& map1, cv::Mat& map2, double fScale) const
{
    cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight());

    cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F);
    cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F);

    for (int v = 0; v < imageSize.height; ++v)
    {
        for (int u = 0; u < imageSize.width; ++u)
        {
            double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale;
            double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale;

            double xi = mParameters.xi();
            double d2 = mx_u * mx_u + my_u * my_u;

            Eigen::Vector3d P;
            P << mx_u, my_u, 1.0 - xi * (d2 + 1.0) / (xi + sqrt(1.0 + (1.0 - xi * xi) * d2));

            Eigen::Vector2d p;
            spaceToPlane(P, p);

            mapX.at<float>(v,u) = p(0);
            mapY.at<float>(v,u) = p(1);
        }
    }

    cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);
}
#endif

cv::Mat OCAMCamera::initUndistortRectifyMap(cv::Mat &map1, cv::Mat &map2,
                                            float fx, float fy,
                                            cv::Size imageSize, float cx,
                                            float cy, cv::Mat rmat) const
{
  if (imageSize == cv::Size(0, 0))
  {
    imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight());
  }

  cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);
  cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);

  Eigen::Matrix3f K_rect;

  K_rect << fx, 0, cx < 0 ? imageSize.width / 2 : cx, 0, fy,
      cy < 0 ? imageSize.height / 2 : cy, 0, 0, 1;

  if (fx < 0 || fy < 0)
  {
    throw std::string(std::string(__FUNCTION__) +
                      ": Focal length must be specified");
  }

  Eigen::Matrix3f K_rect_inv = K_rect.inverse();

  Eigen::Matrix3f R, R_inv;
  cv::cv2eigen(rmat, R);
  R_inv = R.inverse();

  for (int v = 0; v < imageSize.height; ++v)
  {
    for (int u = 0; u < imageSize.width; ++u)
    {
      Eigen::Vector3f xo;
      xo << u, v, 1;

      Eigen::Vector3f uo = R_inv * K_rect_inv * xo;

      Eigen::Vector2d p;
      spaceToPlane(uo.cast<double>(), p);

      mapX.at<float>(v, u) = p(0);
      mapY.at<float>(v, u) = p(1);
    }
  }

  cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false);

  cv::Mat K_rect_cv;
  cv::eigen2cv(K_rect, K_rect_cv);
  return K_rect_cv;
}

int OCAMCamera::parameterCount(void) const
{
  return SCARAMUZZA_CAMERA_NUM_PARAMS;
}

const OCAMCamera::Parameters &OCAMCamera::getParameters(void) const
{
  return mParameters;
}

void OCAMCamera::setParameters(const OCAMCamera::Parameters &parameters)
{
  mParameters = parameters;

  m_inv_scale = 1.0 / (parameters.C() - parameters.D() * parameters.E());
}

void OCAMCamera::readParameters(const std::vector<double> &parameterVec)
{
  if ((int)parameterVec.size() != parameterCount())
  {
    return;
  }

  Parameters params = getParameters();

  params.C() = parameterVec.at(0);
  params.D() = parameterVec.at(1);
  params.E() = parameterVec.at(2);
  params.center_x() = parameterVec.at(3);
  params.center_y() = parameterVec.at(4);
  for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++)
    params.poly(i) = parameterVec.at(5 + i);
  for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
    params.inv_poly(i) = parameterVec.at(5 + SCARAMUZZA_POLY_SIZE + i);

  setParameters(params);
}

void OCAMCamera::writeParameters(std::vector<double> &parameterVec) const
{
  parameterVec.resize(parameterCount());
  parameterVec.at(0) = mParameters.C();
  parameterVec.at(1) = mParameters.D();
  parameterVec.at(2) = mParameters.E();
  parameterVec.at(3) = mParameters.center_x();
  parameterVec.at(4) = mParameters.center_y();
  for (int i = 0; i < SCARAMUZZA_POLY_SIZE; i++)
    parameterVec.at(5 + i) = mParameters.poly(i);
  for (int i = 0; i < SCARAMUZZA_INV_POLY_SIZE; i++)
    parameterVec.at(5 + SCARAMUZZA_POLY_SIZE + i) = mParameters.inv_poly(i);
}

void OCAMCamera::writeParametersToYamlFile(const std::string &filename) const
{
  mParameters.writeToYamlFile(filename);
}

std::string OCAMCamera::parametersToString(void) const
{
  std::ostringstream oss;
  oss << mParameters;

  return oss.str();
}
